From: Robert Lipe Date: Tue, 15 Sep 2015 03:04:53 +0000 (-0500) Subject: Merge remote-tracking branch 'refs/remotes/origin/master' X-Git-Tag: archive/raspbian/1.10.0+ds-2+rpi1~1^2~12^2~10^2~6^2~8 X-Git-Url: https://dgit.raspbian.org/%22http://www.example.com/cgi/%22/%22http:/www.example.com/cgi/%22?a=commitdiff_plain;h=8765b83e67236dbc20f775837773d2ef3cf0fe01;p=gpsbabel.git Merge remote-tracking branch 'refs/remotes/origin/master' --- 8765b83e67236dbc20f775837773d2ef3cf0fe01 diff --cc garmin_gpi.cc index 000000000,17e18f799..cae6f1d42 mode 000000,100644..100644 --- a/garmin_gpi.cc +++ b/garmin_gpi.cc @@@ -1,0 -1,1616 +1,1629 @@@ + /* + + Support for Garmin Points of Interest (.gpi files) + + Copyright (C) 2007 Olaf Klein, o.b.klein@gpsbabel.org + Copyright (C) 2007-2012 Robert Lipe, robertlipe+source@gpsbabel.org + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA + + */ + + /* + History: + + * 2007/05/18: initial release (only a reader) + * 2007/05/20: added writer code with embedded bitmap + * 2007/05/22: add support for multiple bounding boxes + (useful / required!) for large waypoints lists + * 2007/05/23: add optional user bitmap + * 2007/06/02: new method to compute center (mean) of bounds + avoid endless loop in group splitting + * 2007/07/10: put address fields (i.e. city) into GMSD + * 2007/07/12: add write support for new address fields + * 2007/10/20: add option unique + * 2007/12/02: support speed and proximity distance (+ alerts) + * 2008/01/14: fix structure error after adding speed/proximity + * 2008/03/22: add options "speed" and "proximity" (default values) and "sleep" + + ToDo: + + * Display mode ("Symbol & Name") ??? not in gpi ??? + * support category from GMSD "Garmin Special Data" + */ + + #include "defs.h" + #include "cet_util.h" + #include "jeeps/gpsmath.h" + #include "garmin_fs.h" + #include "garmin_gpi.h" + #include + #include + + #define MYNAME "garmin_gpi" + + #define GPI_DBG 1 + #undef GPI_DBG + + #define DEFAULT_ICON "Waypoint" + #define WAYPOINTS_PER_BLOCK 128 + + /* flags used in the gpi address mask */ + #define GPI_ADDR_CITY 1 + #define GPI_ADDR_COUNTRY 2 + #define GPI_ADDR_STATE 4 + #define GPI_ADDR_POSTAL_CODE 8 + #define GPI_ADDR_ADDR 16 + + static char* opt_cat, *opt_pos, *opt_notes, *opt_hide_bitmap, *opt_descr, *opt_bitmap; + static char* opt_unique, *opt_alerts, *opt_units, *opt_speed, *opt_proximity, *opt_sleep; + static char* opt_writecodec; + static double defspeed, defproximity; + static int alerts; + + static arglist_t garmin_gpi_args[] = { + { + "alerts", &opt_alerts, "Enable alerts on speed or proximity distance", + NULL, ARGTYPE_BOOL, ARG_NOMINMAX + }, + { + "bitmap", &opt_bitmap, "Use specified bitmap on output", + NULL, ARGTYPE_FILE, ARG_NOMINMAX + }, + { + "category", &opt_cat, "Default category on output", + "My points", ARGTYPE_STRING, ARG_NOMINMAX + }, + { + "hide", &opt_hide_bitmap, "Don't show gpi bitmap on device", + NULL, ARGTYPE_BOOL, ARG_NOMINMAX + }, + { + "descr", &opt_descr, "Write description to address field", + NULL, ARGTYPE_BOOL, ARG_NOMINMAX + }, + { + "notes", &opt_notes, "Write notes to address field", + NULL, ARGTYPE_BOOL, ARG_NOMINMAX + }, + { + "position", &opt_pos, "Write position to address field", + NULL, ARGTYPE_BOOL, ARG_NOMINMAX + }, + { + "proximity", &opt_proximity, "Default proximity", + NULL, ARGTYPE_STRING, ARG_NOMINMAX + }, + { + "sleep", &opt_sleep, "After output job done sleep n second(s)", + NULL, ARGTYPE_INT, "1", NULL + }, + { + "speed", &opt_speed, "Default speed", + NULL, ARGTYPE_STRING, ARG_NOMINMAX + }, + { + "unique", &opt_unique, "Create unique waypoint names (default = yes)", + "Y", ARGTYPE_BOOL, ARG_NOMINMAX + }, + { + "units", &opt_units, "Units used for names with @speed ('s'tatute or 'm'etric)", + "m", ARGTYPE_STRING, ARG_NOMINMAX + }, + { + "writecodec", &opt_writecodec, "codec to use for writing strings", + "windows-1252", ARGTYPE_STRING, ARG_NOMINMAX + }, + ARG_TERMINATOR + }; + + typedef struct { + public: + int D2; + char S3[9]; /* "GRMRECnn" */ + time_t crdate; /* creation date and time */ + char POI[4]; /* "POI" */ + char S8[3]; + QString group; + QString category; + } reader_data_t; + + typedef struct writer_data_s { + queue Q; + int ct; + int sz; + int alert; + bounds bds; + struct writer_data_s* top_left; + struct writer_data_s* top_right; + struct writer_data_s* buttom_left; + struct writer_data_s* buttom_right; + } writer_data_t; + + typedef struct gpi_waypt_data_s { + int sz; + char* addr; + char* postal_code; + } gpi_waypt_data_t; + + typedef struct { + int32_t size; + int16_t res1; + int16_t res2; + int32_t image_offset; + int32_t header_size; + int32_t width; + int32_t height; + int16_t planes; + int16_t bpp; + int32_t compression_type; + int32_t image_data_size; + int32_t resolution_h; + int32_t resolution_v; + int32_t used_colors; + int32_t important_colors; + } bmp_header_t; + + typedef struct { + int16_t index; + int16_t height; + int16_t width; + int16_t line_sz; + int16_t bpp; + int16_t fixed_0; + int32_t image_size; + int32_t fixed_2c; + int32_t flag1; + int32_t tr_color; + int32_t flag2; + int32_t size_2c; + } gpi_bitmap_header_t; + + typedef struct { + int sz; + int alerts; + short mask; + char addr_is_dynamic; + char* addr; + char* city; + char* country; + char* phone_nr; + char* postal_code; + char* state; + } gpi_waypt_t; + + static gbfile* fin, *fout; + static int16_t codepage; /* code-page, i.e. 1252 */ + static reader_data_t* rdata; + static writer_data_t* wdata; + static short_handle short_h; + static char units; + static time_t gpi_timestamp = 0; + + #ifdef GPI_DBG + # define PP warning("@%1$6x (%1$8d): ", gbftell(fin)) + # define dbginfo warning + #else + # define PP + #endif + + /******************************************************************************* + * %%% gpi reader %%% * + *******************************************************************************/ + + /* look for or initialize GMSD */ + static garmin_fs_t* + gpi_gmsd_init(Waypoint* wpt) + { + garmin_fs_t* gmsd = GMSD_FIND(wpt); + if (wpt == NULL) { + fatal(MYNAME ": Error in file structure.\n"); + } + if (gmsd == NULL) { + gmsd = garmin_fs_alloc(-1); + fs_chain_add(&wpt->fs, (format_specific_data*) gmsd); + } + return gmsd; + } + + /* read a standard string with or without 'EN' (or whatever) header */ + static char* + gpi_read_string_old(const char* field) + { + int l1; + char* res = NULL; + + l1 = gbfgetint16(fin); + if (l1 > 0) { + short l2; + char first; + + first = gbfgetc(fin); + if (first == 0) { + char en[2]; + + is_fatal((gbfgetc(fin) != 0), + MYNAME ": Error reading field '%s'!", field); + + gbfread(en, 1, sizeof(en), fin); + l2 = gbfgetint16(fin); + is_fatal((l2 + 4 != l1), + MYNAME ": Error out of sync (wrong size %d/%d) on field '%s'!", l1, l2, field); + + if ((en[0] < 'A') || (en[0] > 'Z') || (en[1] < 'A') || (en[1] > 'Z')) { + fatal(MYNAME ": Invalid country code!\n"); + } + res = (char*) xmalloc(l2 + 1); + res[l2] = '\0'; + PP; + if (l2 > 0) { + gbfread(res, 1, l2, fin); + } + } else { + res = (char*) xmalloc(l1 + 1); + *res = first; + *(res + l1) = '\0'; + PP; + l1--; + if (l1 > 0) { + gbfread(res + 1, 1, l1, fin); + } + + } + } + #ifdef GPI_DBG + dbginfo("%s: %s\n", field, (res == NULL) ? "" : res); + #endif + return res; + } + + static QString + gpi_read_string(const char* field) + { + char*s = gpi_read_string_old(field); + QString rv = STRTOUNICODE(s); + xfree(s); + return rv; + } + + static void + read_header(void) + { + int len, i; + #ifdef GPI_DBG + struct tm tm; + char stime[32]; + #endif + + i = gbfgetint32(fin); + if (i != 0) { + i = gbfgetint32(fin); + } + rdata->D2 = gbfgetint32(fin); + + gbfread(&rdata->S3, 1, sizeof(rdata->S3) - 1, fin); /* GRMRECnn */ + if (strncmp(rdata->S3, "GRMREC", 6) != 0) { + fatal(MYNAME ": No GPI file!\n"); + } + + PP; + rdata->crdate = gbfgetint32(fin); + #ifdef GPI_DBG + tm = *localtime(&rdata->crdate); + tm.tm_year += 20; /* !!! */ + tm.tm_mday -= 1; /* !!! */ + strftime(stime, sizeof(stime), "%Y/%m/%d %H:%M:%S", &tm); + dbginfo("crdate = %lu (%s)\n", rdata->crdate, stime); + #endif + + (void) gbfgetint16(fin); /* 0 */ + + len = gbfgetint16(fin); + gbfseek(fin, len, SEEK_CUR); /* "my.gpi" */ + + i = gbfgetint32(fin); /* 1 */ + (void) gbfgetint32(fin); /* 12 */ + /* There are two dwords next. On most typical files, they're + * "1" and "12". On files from garminoneline.de/extras/poi, the + * next two words are "15" and "5" and there's 17 additional bytes + * that I can't identify. So hardcode a seek here for now. + */ + if (i == 15) { + gbfseek(fin, 17, SEEK_CUR); + } + + gbfread(&rdata->POI, 1, sizeof(rdata->POI) - 1, fin); + + if (strncmp(rdata->POI, "POI", 3) != 0) { + fatal(MYNAME ": Wrong or unsupported GPI file!\n"); + } + + for (i = 0; i < 3; i++) { + (void)gbfgetc(fin); + } + gbfread(&rdata->S8, 1, sizeof(rdata->S8) - 1, fin); + + codepage = gbfgetint16(fin); + (void) gbfgetint16(fin); /* typically 0, but 0x11 in + Garminonline.de files. */ + + #ifdef GPI_DBG + PP; + dbginfo("< leaving header\n"); + #endif + } + + /* gpi tag handler */ + static int read_tag(const char* caller, const int tag, Waypoint* wpt); + + + /* read a single poi with all options */ + static void + read_poi(const int sz, const int tag) + { + int pos, len; + Waypoint* wpt; + + #ifdef GPI_DBG + PP; + dbginfo("> reading poi (size %d)\n", sz); + #endif + PP; + len = 0; + if (tag == 0x80002) { + len = gbfgetint32(fin); /* sub-header size */ + } + #ifdef GPI_DBG + dbginfo("poi sublen = %1$d (0x%1$x)\n", len); + #endif + (void) len; + pos = gbftell(fin); + + wpt = new Waypoint; + wpt->icon_descr = DEFAULT_ICON; + + wpt->latitude = GPS_Math_Semi_To_Deg(gbfgetint32(fin)); + wpt->longitude = GPS_Math_Semi_To_Deg(gbfgetint32(fin)); + + (void) gbfgetint16(fin); /* ? always 1 ? */ + (void) gbfgetc(fin); /* seems to 1 when extra options present */ + wpt->shortname = gpi_read_string("Shortname"); + + while (gbftell(fin) < (gbsize_t)(pos + sz - 4)) { + int tag = gbfgetint32(fin); + if (! read_tag("read_poi", tag, wpt)) { + break; + } + } + + if (wpt->description.isEmpty() && !wpt->notes.isEmpty()) { + wpt->description = wpt->notes; + } + if (wpt->notes.isEmpty() && !wpt->description.isEmpty()) { + wpt->notes = wpt->description; + } + + waypt_add(wpt); + + #ifdef GPI_DBG + PP; + dbginfo("< leaving poi\n"); + #endif + } + + /* read poi's following a group header */ + static void + read_poi_list(const int sz) + { + int pos, i; + + pos = gbftell(fin); + #ifdef GPI_DBG + PP; + dbginfo("> reading poi list (-> %1$x / %1$d )\n", pos + sz); + #endif + PP; + i = gbfgetint32(fin); /* mostly 23 (0x17) */ + #ifdef GPI_DBG + dbginfo("list sublen = %1$d (0x%1$x)\n", i); + #else + (void) i; + #endif + (void) gbfgetint32(fin); /* max-lat */ + (void) gbfgetint32(fin); /* max-lon */ + (void) gbfgetint32(fin); /* min-lat */ + (void) gbfgetint32(fin); /* min-lon */ + + (void) gbfgetc(fin); /* three unknown bytes */ + (void) gbfgetc(fin); /* ? should be zero ? */ + (void) gbfgetc(fin); + + (void) gbfgetint32(fin); /* ? const 0x1000100 ? */ + + while (gbftell(fin) < (gbsize_t)(pos + sz - 4)) { + int tag = gbfgetint32(fin); + if (! read_tag("read_poi_list", tag, NULL)) { + return; + } + } + #ifdef GPI_DBG + PP; + dbginfo("< leaving poi list\n"); + #endif + } + + + static void + read_poi_group(const int sz, const int tag) + { + int pos; + + pos = gbftell(fin); + #ifdef GPI_DBG + PP; + dbginfo("> reading poi group (-> %1$x / %1$d)\n", pos + sz); + #endif + if (tag == 0x80009) { + int subsz; + + PP; + subsz = gbfgetint32(fin); /* ? offset to category data ? */ + #ifdef GPI_DBG + dbginfo("group sublen = %d (-> %x / %d)\n", subsz, pos + subsz + 4, pos + subsz + 4); + #else + (void)subsz; + #endif + } + rdata->group = gpi_read_string("Group"); + + while (gbftell(fin) < (gbsize_t)(pos + sz)) { + int subtag = gbfgetint32(fin); + if (! read_tag("read_poi_group", subtag, NULL)) { + break; + } + } + + #ifdef GPI_DBG + PP; + dbginfo("< leaving poi group\n"); + #endif + } + + // TODO: 'tag' is probably not a 32 bit value. + // most likely it's a pair of 16's: the first pair is the tag number. + // if the second 16 is "eight", then it's an + // extended thingy and it has a 4-byte extended record length (total number + // of bytes for all record fields and all nested records, starting after the + // length field) + /* gpi tag handler */ + static int + read_tag(const char* caller, const int tag, Waypoint* wpt) + { + int pos, sz, dist; + double speed; + short mask; + QString str; + char* cstr; + garmin_fs_t* gmsd; + + sz = gbfgetint32(fin); + pos = gbftell(fin); + + #ifdef GPI_DBG + PP; + dbginfo("%s: tag = 0x%x (size %d)\n", caller, tag, sz); + #endif + if ((tag >= 0x80000) && (tag <= 0x800ff)) { + sz += 4; + } + + switch (tag) { + case 0x3: /* size = 12 */ + case 0x80003: /* size = 12 */ + + dist = gbfgetint16(fin); /* proximity distance in meters */ + speed = (double)gbfgetint16(fin) / 100; /* speed in meters per second */ + + if (dist > 0) { + WAYPT_SET(wpt, proximity, dist); + } + if (speed > 0) { + /* speed isn't part of a normal waypoint + WAYPT_SET(wpt, speed, speed); + */ + if ((wpt->shortname.isEmpty() || ((wpt->shortname).indexOf('@'))==-1)) { + if (units == 's') { + speed = MPS_TO_MPH(speed); + } else { + speed = MPS_TO_KPH(speed); + } + QString base = wpt->shortname.isEmpty() ? "WPT" : wpt->shortname; + wpt->shortname = base + QString("@%1").arg(speed,0,'f',0); + } + } + + (void) gbfgetint32(fin); + (void) gbfgetint32(fin); + break; + + case 0x4: /* size = 2 ? */ + case 0x6: /* size = 2 ? */ + break; + + case 0x5: /* group bitmap */ + break; + + case 0x7: + (void) gbfgetint16(fin); /* category number */ + rdata->category = gpi_read_string("Category"); + break; + + case 0xa: + wpt->description = gpi_read_string("Description"); + break; + + case 0xe: /* ? notes or description / or both ? */ + mask = gbfgetc(fin); + // Olaf's code called this a mask, but the bits below have nothing + // in common. I'm wondering if that first byte is something else and + // a type e is always a note. + switch (mask) { + case 0x01: + case 0x05: + case 0x32: + str = gpi_read_string("Notes"); + default: + break; + } + + if (!wpt->description.isEmpty()) { + wpt->notes = str; + } else { + wpt->description = str; + } + break; + + case 0x2: + case 0x80002: + read_poi(sz, tag); + break; + + case 0x80008: + read_poi_list(sz); + break; + + case 0x9: /* ? older versions / no category data ? */ + case 0x80009: /* current POI loader */ + read_poi_group(sz, tag); + break; + + case 0x8000b: /* address (street/city...) */ + (void) gbfgetint32(fin); + // FALLTHROUGH + case 0xb: /* as seen in German POI files. */ + PP; + mask = gbfgetint16(fin); /* address fields mask */ + #ifdef GPI_DBG + dbginfo("GPI Address field mask: %d (0x%02x)\n", mask, mask); + #endif + if ((mask & GPI_ADDR_CITY) && (cstr = gpi_read_string_old("City"))) { + gmsd = gpi_gmsd_init(wpt); + GMSD_SET(city, cstr); + } + if ((mask & GPI_ADDR_COUNTRY) && (cstr = gpi_read_string_old("Country"))) { + gmsd = gpi_gmsd_init(wpt); + GMSD_SET(country, cstr); + } + if ((mask & GPI_ADDR_STATE) && (cstr = gpi_read_string_old("State"))) { + gmsd = gpi_gmsd_init(wpt); + GMSD_SET(state, cstr); + } + if ((mask & GPI_ADDR_POSTAL_CODE) && (cstr = gpi_read_string_old("Postal code"))) { + gmsd = gpi_gmsd_init(wpt); + GMSD_SET(postal_code, cstr); + } + if ((mask & GPI_ADDR_ADDR) && (cstr = gpi_read_string_old("Street address"))) { + gmsd = gpi_gmsd_init(wpt); + GMSD_SET(addr, cstr); + } + break; + + case 0xc: + mask = gbfgetint16(fin); + if ((mask & 1) && (cstr = gpi_read_string_old("Phone"))) { + gmsd = gpi_gmsd_init(wpt); + GMSD_SET(phone_nr, cstr); + } + if ((mask & 2) && (cstr = gpi_read_string_old("Phone2"))) { + gmsd = gpi_gmsd_init(wpt); + GMSD_SET(phone_nr2, cstr); + } + if ((mask & 4) && (cstr = gpi_read_string_old("Fax"))) { + gmsd = gpi_gmsd_init(wpt); + GMSD_SET(fax_nr, cstr); + } + if ((mask & 8) && (cstr = gpi_read_string_old("Email"))) { + gmsd = gpi_gmsd_init(wpt); + GMSD_SET(email, cstr); + } + if ((mask & 0x10) && (str = gpi_read_string("Link"), !str.isEmpty())) { + waypt_add_url(wpt, str, str); + } + break; + + case 0x8000c: /* phone-number */ + (void) gbfgetint32(fin); + PP; + + mask = gbfgetint16(fin); /* phone fields mask */ + #ifdef GPI_DBG + dbginfo("GPI Phone field mask: %d (0x%02x)\n", mask, mask); + #endif + if ((mask & 1) && (cstr = gpi_read_string_old("Phone"))) { + gmsd = gpi_gmsd_init(wpt); + GMSD_SET(phone_nr, cstr); + } + break; + + case 0x80012: /* ? sounds / images ? */ + break; + + /* Images? Seen in http://geepeeex.com/Stonepages.gpi */ + case 0xd: + break; + + case 0x11: + case 0x80007: + /* Looks like some kind of calendar information. */ + #ifdef GPI_DBG + { + int x; + unsigned char* b = (unsigned char*) xmalloc(sz); + fprintf(stderr, "Tag: %x\n", tag); + gbfread(b, 1, sz, fin); + fprintf(stderr, "\n"); + for (x = 0; x < sz; x++) { + fprintf(stderr, "%02x ", b[x]); + } + fprintf(stderr, "\n"); + for (x = 0; x < sz; x++) { + fprintf(stderr, "%c", isalnum(b[x]) ? b[x] : '.'); + } + fprintf(stderr, "\n"); + } + #endif // GPI_DBG + break; + default: + warning(MYNAME ": Unknown tag (0x%x). Please report!\n", tag); + return 0; + } + gbfseek(fin, pos + sz, SEEK_SET); + return 1; + } + + /******************************************************************************* + * %%% gpi writer %%% * + *******************************************************************************/ + + static void + write_string(const char* str, const char long_format) + { + int len; + + len = strlen(str); + if (long_format) { + gbfputint32(len + 4, fout); + gbfwrite("EN", 1, 2, fout); + } + gbfputint16(len, fout); + gbfwrite(str, 1, len, fout); + } + + static void + write_string(const QString& str, const char long_format) + { + write_string(STRFROMUNICODE(str), long_format); + } + + + static int + compare_wpt_cb(const queue* a, const queue* b) + { + const Waypoint* wa = (Waypoint*) a; + const Waypoint* wb = (Waypoint*) b; + return wa->shortname.compare(wb->shortname); + } + + static char + compare_strings(const QString& s1, const QString& s2) + { + return s1.compare(s2); + } + + static writer_data_t* + wdata_alloc() + { + writer_data_t* res; + + res = (writer_data_t*) xcalloc(1, sizeof(*res)); + QUEUE_INIT(&res->Q); + waypt_init_bounds(&res->bds); + + return res; + } + + + static void + wdata_free(writer_data_t* data) + { + queue* elem, *tmp; + + QUEUE_FOR_EACH(&data->Q, elem, tmp) { + Waypoint* wpt = (Waypoint*)elem; + + if (wpt->extra_data) { + gpi_waypt_t* dt = (gpi_waypt_t*) wpt->extra_data; + if (dt->addr_is_dynamic) { + xfree(dt->addr); + } + xfree(dt); + } + delete wpt; + } + + if (data->top_left) { + wdata_free(data->top_left); + } + if (data->top_right) { + wdata_free(data->top_right); + } + if (data->buttom_left) { + wdata_free(data->buttom_left); + } + if (data->buttom_right) { + wdata_free(data->buttom_right); + } + + xfree(data); + } + + + static void + wdata_add_wpt(writer_data_t* data, Waypoint* wpt) + { + data->ct++; + ENQUEUE_TAIL(&data->Q, &wpt->Q); + waypt_add_to_bounds(&data->bds, wpt); + } + + + static void + wdata_check(writer_data_t* data) + { + queue* elem, *tmp; + double center_lat, center_lon; + + if ((data->ct <= WAYPOINTS_PER_BLOCK) || + /* avoid endless loop for points (more than WAYPOINTS_PER_BLOCK) + at same coordinates */ + ((data->bds.min_lat >= data->bds.max_lat) && (data->bds.min_lon >= data->bds.max_lon))) { + if (data->ct > 1) { + sortqueue(&data->Q, compare_wpt_cb); + } + return; + } + + /* compute the (mean) center of current bounds */ + + center_lat = center_lon = 0; + QUEUE_FOR_EACH(&data->Q, elem, tmp) { + Waypoint* wpt = (Waypoint*) elem; + center_lat += wpt->latitude; + center_lon += wpt->longitude; + } + center_lat /= data->ct; + center_lon /= data->ct; + + QUEUE_FOR_EACH(&data->Q, elem, tmp) { + Waypoint* wpt = (Waypoint*) elem; + writer_data_t** ref; + + if (wpt->latitude < center_lat) { + if (wpt->longitude < center_lon) { + ref = &data->buttom_left; + } else { + ref = &data->buttom_right; + } + } else { + if (wpt->longitude < center_lon) { + ref = &data->top_left; + } else { + ref = &data->top_right; + } + } + + if (*ref == NULL) { + *ref = wdata_alloc(); + } + + data->ct--; + dequeue(&wpt->Q); + + wdata_add_wpt(*ref, wpt); + } + + if (data->top_left) { + wdata_check(data->top_left); + } + if (data->top_right) { + wdata_check(data->top_right); + } + if (data->buttom_left) { + wdata_check(data->buttom_left); + } + if (data->buttom_right) { + wdata_check(data->buttom_right); + } + } + + + static int + wdata_compute_size(writer_data_t* data) + { + queue* elem, *tmp; - int res; ++ int res = 0; ++ ++ if (QUEUE_EMPTY(&data->Q)) ++ goto skip_empty_block; /* do not issue an empty block */ + + res = 23; /* bounds, ... of tag 0x80008 */ + + QUEUE_FOR_EACH(&data->Q, elem, tmp) { + Waypoint* wpt = (Waypoint*) elem; + gpi_waypt_t* dt; + garmin_fs_t* gmsd; + QString str; + + res += 12; /* tag/sz/sub-sz */ + res += 19; /* poi fixed size */ + res += wpt->shortname.length(); + if (! opt_hide_bitmap) { + res += 10; /* tag(4) */ + } + + dt = (gpi_waypt_t*) xcalloc(1, sizeof(*dt)); + wpt->extra_data = dt; + + if (alerts) { + #if NEW_STRINGS + // examine closely. + const char* pos; + int pidx; + if ((pidx = wpt->shortname.indexOf('@')) != -1) { + pos = CSTR(wpt->shortname.mid(pidx)); + #else + char* pos; + if ((pos = strchr(wpt->shortname, '@'))) { + #endif + double speed, scale; + if (units == 's') { + scale = MPH_TO_MPS(1); + } else { + scale = KPH_TO_MPS(1); + } + parse_speed(pos + 1, &speed, scale, MYNAME); + if (speed > 0) { + WAYPT_SET(wpt, speed, speed); + } + #if 0 + if (pos > wpt->shortname) { + wpt->shortname[pos - wpt->shortname] = '\0'; + } + #endif + } else if ((opt_speed) && (! WAYPT_HAS(wpt, speed))) { + WAYPT_SET(wpt, speed, defspeed); + } + + if ((opt_proximity) && (! WAYPT_HAS(wpt, proximity))) { + WAYPT_SET(wpt, proximity, defproximity); + } + + if ((WAYPT_HAS(wpt, speed) && (wpt->speed > 0)) || + (WAYPT_HAS(wpt, proximity) && (wpt->proximity > 0))) { + data->alert = 1; + dt->alerts++; + res += 20; /* tag(3) */ + } + } + + str = QString(); + if (opt_descr) { + if (!wpt->description.isEmpty()) { + str = xstrdup(wpt->description); + } + } else if (opt_notes) { + if (!wpt->notes.isEmpty()) { + str = xstrdup(wpt->notes); + } + } else if (opt_pos) { + str = pretty_deg_format(wpt->latitude, wpt->longitude, 's', " ", 0); + } + + + if (!str.isEmpty()) { + dt->addr_is_dynamic = 1; + dt->addr = xstrdup(str); + dt->mask |= GPI_ADDR_ADDR; + dt->sz += (8 + strlen(dt->addr)); + } + + if ((gmsd = GMSD_FIND(wpt))) { + if ((dt->mask == 0) && ((dt->addr = GMSD_GET(addr, NULL)))) { + dt->mask |= GPI_ADDR_ADDR; + dt->sz += (8 + strlen(dt->addr)); + } + if ((dt->city = GMSD_GET(city, NULL))) { + dt->mask |= GPI_ADDR_CITY; + dt->sz += (8 + strlen(dt->city)); + } + if ((dt->country = GMSD_GET(country, NULL))) { + dt->mask |= GPI_ADDR_COUNTRY; + dt->sz += (8 + strlen(dt->country)); + } + if ((dt->state = GMSD_GET(state, NULL))) { + dt->mask |= GPI_ADDR_STATE; + dt->sz += (8 + strlen(dt->state)); + } + if ((dt->postal_code = GMSD_GET(postal_code, NULL))) { + dt->mask |= GPI_ADDR_POSTAL_CODE; + dt->sz += (2 + strlen(dt->postal_code)); /* short form */ + } + + if ((dt->phone_nr = GMSD_GET(phone_nr, NULL))) { + res += (12 + 4 + strlen(dt->phone_nr)); + } + } + if (dt->mask) { + dt->sz += 2; /* + mask (two bytes) */ + } + if (dt->sz) { + res += (dt->sz + 12); /* + header size */ + } + + str = wpt->description; + if (str.isEmpty()) { + str = wpt->notes; + } + // if (str && (strcmp(str, wpt->shortname) == 0)) str = NULL; + if (!str.isEmpty()) { + res += (12 + 4 + str.length()); + } + } + ++skip_empty_block: ++ + if (data->top_left) { + res += wdata_compute_size(data->top_left); + } + if (data->top_right) { + res += wdata_compute_size(data->top_right); + } + if (data->buttom_left) { + res += wdata_compute_size(data->buttom_left); + } + if (data->buttom_right) { + res += wdata_compute_size(data->buttom_right); + } + + data->sz = res; + ++ if (QUEUE_EMPTY(&data->Q)) ++ return res; ++ + return res + 12; /* + 12 = caller needs info about tag header size */ + } + + + static void + wdata_write(const writer_data_t* data) + { + queue* elem, *tmp; + ++ if (QUEUE_EMPTY(&data->Q)) ++ goto skip_empty_block; /* do not issue an empty block */ ++ + gbfputint32(0x80008, fout); + gbfputint32(data->sz, fout); + gbfputint32(23, fout); /* bounds + three bytes */ + + gbfputint32(GPS_Math_Deg_To_Semi(data->bds.max_lat), fout); + gbfputint32(GPS_Math_Deg_To_Semi(data->bds.max_lon), fout); + gbfputint32(GPS_Math_Deg_To_Semi(data->bds.min_lat), fout); + gbfputint32(GPS_Math_Deg_To_Semi(data->bds.min_lon), fout); + + gbfputint32(0, fout); + gbfputint16(1, fout); + gbfputc(data->alert, fout); + + QUEUE_FOR_EACH(&data->Q, elem, tmp) { + QString str; + int s0, s1; + Waypoint* wpt = (Waypoint*)elem; + gpi_waypt_t* dt = (gpi_waypt_t*) wpt->extra_data; + + str = wpt->description; + if (str.isEmpty()) { + str = wpt->notes; + } + + gbfputint32(0x80002, fout); + s0 = s1 = 19 + wpt->shortname.length(); + if (! opt_hide_bitmap) { + s0 += 10; /* tag(4) */ + } + if (!str.isEmpty()) { + s0 += (12 + 4 + str.length()); /* descr */ + } + if (dt->sz) { + s0 += (12 + dt->sz); /* address part */ + } + if (dt->phone_nr) { + s0 += (12 + 4 + strlen(dt->phone_nr)); + } + if (dt->alerts) { + s0 += 20; /* tag(3) */ + } + + gbfputint32(s0, fout); /* size of following data (tag) */ + gbfputint32(s1, fout); /* basic size (without options) */ + + gbfputint32(GPS_Math_Deg_To_Semi(wpt->latitude), fout); + gbfputint32(GPS_Math_Deg_To_Semi(wpt->longitude), fout); + + gbfputint16(1, fout); /* ? always 1 ? */ + gbfputc(alerts, fout); /* seems to be 1 when extra options present */ + + write_string(wpt->shortname, 1); + + if (dt->alerts) { + char flag = 0; + + gbfputint32(3, fout); /* tag(3) */ + gbfputint32(12, fout); /* always 12 */ + + if (WAYPT_HAS(wpt, proximity) && (wpt->proximity > 0)) { + gbfputint16((int) wpt->proximity, fout); + flag = 4; + } else { + gbfputint16(0, fout); + } + if (WAYPT_HAS(wpt, speed) && (wpt->speed > 0)) { + gbfputint16((int)(wpt->speed * 100), fout); + flag = 5; + } else { + gbfputint16(0, fout); + } + + gbfputint32(0x100100, fout); /* ??? */ + gbfputc(1, fout); /* ??? */ + gbfputc(1, fout); /* ??? */ + gbfputc(flag, fout); + gbfputc(0x10, fout); /* ??? */ + } + + if (! opt_hide_bitmap) { + gbfputint32(4, fout); /* tag(4) */ + gbfputint32(2, fout); /* ? always 2 == version ??? */ + gbfputint16(0, fout); + } + + if (!str.isEmpty()) { + gbfputint32(0xa, fout); + gbfputint32(str.length() + 8, fout); /* string + string header */ + write_string(str, 1); + } + + if (dt->sz) { /* gpi address */ + gbfputint32(0x8000b, fout); + gbfputint32(dt->sz, fout); + gbfputint32(0x2, fout); /* ? always 2 ? */ + gbfputint16(dt->mask, fout); + if (dt->mask & GPI_ADDR_CITY) { + write_string(dt->city, 1); + } + if (dt->mask & GPI_ADDR_COUNTRY) { + write_string(dt->country, 1); + } + if (dt->mask & GPI_ADDR_STATE) { + write_string(dt->state, 1); + } + if (dt->mask & GPI_ADDR_POSTAL_CODE) { + write_string(dt->postal_code, 0); + } + if (dt->mask & GPI_ADDR_ADDR) { + write_string(dt->addr, 1); + } + } + + if (dt->phone_nr) { + gbfputint32(0x8000c, fout); + gbfputint32(strlen(dt->phone_nr) + 2 + 2, fout); + gbfputint32(0x2, fout); /* ? always 2 ? */ + gbfputint16(1, fout); /* mask */ + write_string(dt->phone_nr, 0); + } + } + ++skip_empty_block: ++ + if (data->top_left) { + wdata_write(data->top_left); + } + if (data->top_right) { + wdata_write(data->top_right); + } + if (data->buttom_left) { + wdata_write(data->buttom_left); + } + if (data->buttom_right) { + wdata_write(data->buttom_right); + } + } + + + static void + write_category(const char* category, const unsigned char* image, const int image_sz) + { + int sz; + + sz = wdata_compute_size(wdata); + sz += 8; /* string header */ + sz += strlen(opt_cat); + + gbfputint32(0x80009, fout); + if ((! opt_hide_bitmap) && image_sz) { + gbfputint32(sz + image_sz + 8, fout); + } else { + gbfputint32(sz, fout); + } + gbfputint32(sz, fout); + write_string(opt_cat, 1); + + wdata_write(wdata); + + if ((! opt_hide_bitmap) && image_sz) { + gbfputint32(5, fout); + gbfputint32(image_sz, fout); + gbfwrite(image, 1, image_sz, fout); + } + } + + + static void + write_header(void) + { + time_t time = gpi_timestamp; + + if (time != 0) { + struct tm tm; + tm = *gmtime(&time); + tm.tm_year -= 20; + time = mkgmtime(&tm); + time += SECONDS_PER_DAY; + } + + gbfputint32(0, fout); + gbfputint32(0x16, fout); + gbfwrite("GRMREC00", 1, 8, fout); + gbfputint32(time, fout); + gbfputint16(0, fout); + gbfputint16(6, fout); + gbfwrite("my.gpi", 1, 6, fout); + gbfputint32(1, fout); + gbfputint32(0xc, fout); + gbfwrite("POI", 1, 3, fout); + gbfputc(0, fout); + gbfputc(0, fout); + gbfputc(0, fout); + gbfwrite("00", 1, 2, fout); + gbfputint16(codepage, fout); + gbfputint16(0, fout); + } + + + static void + enum_waypt_cb(const Waypoint* ref) + { + Waypoint* wpt; + queue* elem, *tmp; + + QUEUE_FOR_EACH(&wdata->Q, elem, tmp) { + Waypoint* cmp = (Waypoint*) elem; + + /* sort out nearly equal waypoints */ + if ((compare_strings(cmp->shortname, ref->shortname) == 0) && + (cmp->latitude == ref->latitude) && + (cmp->longitude == ref->longitude) && + (compare_strings(cmp->description, ref->description) == 0) && + (compare_strings(cmp->notes, ref->notes) == 0)) { + return; + } + } + + wpt = new Waypoint(*ref); + + if (*opt_unique == '1') { + wpt->shortname = mkshort(short_h, wpt->shortname); + } + + wdata_add_wpt(wdata, wpt); + } + + + static void + load_bitmap_from_file(const char* fname, unsigned char** data, int* data_sz) + { + gbfile* f; + int i, sz; + int dest_bpp; + int src_line_sz, dest_line_sz; + bmp_header_t src_h; + int* color_table = NULL; + gpi_bitmap_header_t* dest_h; + unsigned char* ptr; + + f = gbfopen_le(fname, "rb", MYNAME); + is_fatal(gbfgetint16(f) != 0x4d42, MYNAME ": No BMP image."); + + /* read a standard bmp file header */ + src_h.size = gbfgetint32(f); + src_h.res1 = gbfgetint16(f); + src_h.res2 = gbfgetint16(f); + src_h.image_offset = gbfgetint32(f); + src_h.header_size = gbfgetint32(f); + src_h.width = gbfgetint32(f); + src_h.height = gbfgetint32(f); + src_h.planes = gbfgetint16(f); + src_h.bpp = gbfgetint16(f); + src_h.compression_type = gbfgetint32(f); + src_h.image_data_size = gbfgetint32(f); + src_h.resolution_h = gbfgetint32(f); + src_h.resolution_v = gbfgetint32(f); + src_h.used_colors = gbfgetint32(f); + src_h.important_colors = gbfgetint32(f); + + /* Workaround for indexed BMP's with used_colors = 0 */ + if ((src_h.bpp == 8) && (src_h.used_colors == 0)) { + src_h.used_colors = (src_h.image_offset - gbftell(f)) / 4; + } + + #ifdef GPI_DBG + printf("data size: 0x%1$x (%1$d)\n", src_h.size); + printf("image data offset: 0x%1$x (%1$d)\n", src_h.image_offset); + printf("header size: 0x%1$x (%1$d)\n", src_h.header_size); + printf("image width: 0x%1$x (%1$d)\n", src_h.width); + printf("image height: 0x%1$x (%1$d)\n", src_h.height); + printf("number of planes: 0x%1$x (%1$d)\n", src_h.planes); + printf("bits per pixel: 0x%1$x (%1$d)\n", src_h.bpp); + printf("compression type: 0x%1$x (%1$d)\n", src_h.compression_type); + printf("image size: 0x%1$x (%1$d)\n", src_h.image_data_size); + printf("horizontal resolution: 0x%1$x (%1$d)\n", src_h.resolution_h); + printf("vertical resolution: 0x%1$x (%1$d)\n", src_h.resolution_v); + printf("number of colors: 0x%1$x (%1$d)\n", src_h.used_colors); + printf("important colors: 0x%1$x (%1$d)\n", src_h.important_colors); + #endif + + /* sort out unsupported files */ + if (!((src_h.width <= 24) && (src_h.height <= 24) && + (src_h.width > 0) && (src_h.height > 0))) { + fatal(MYNAME ": Unsupported format (%dx%d)!\n", src_h.width, src_h.height); + } + if (!((src_h.bpp == 8) || (src_h.bpp == 24) || (src_h.bpp == 32))) { + fatal(MYNAME ": Unsupported color depth (%d)!\n", src_h.bpp); + } + if (!(src_h.compression_type == 0)) { + fatal(MYNAME ": Sorry, we don't support compressed bitmaps.\n"); + } + + if (src_h.used_colors > 0) { + color_table = (int*) xmalloc(4 * src_h.used_colors); + gbfread(color_table, 1, 4 * src_h.used_colors, f); + for (i = 0; i < src_h.used_colors; i++) { + int color = color_table[i]; + /* swap blue and red value */ + color = (color >> 16) | (color << 16) | (color & 0x00ff00); + color_table[i] = color & 0xffffff; + } + } + + /* calculate line-size for source and destination */ + src_line_sz = (src_h.width * src_h.bpp) / 8; + src_line_sz = ((int)((src_line_sz + 3) / 4)) * 4; + + if (src_h.bpp == 24) { + dest_bpp = 32; + } else { + dest_bpp = src_h.bpp; + } + + dest_line_sz = (src_h.width * dest_bpp) / 8; + dest_line_sz = ((int)((dest_line_sz + 3) / 4)) * 4; + + sz = sizeof(*dest_h) + (src_h.height * dest_line_sz); + if (src_h.used_colors) { + sz += (src_h.used_colors * 4); + } + + ptr = (unsigned char*) xmalloc(sz); + dest_h = (gpi_bitmap_header_t*)ptr; + *data = ptr; + *data_sz = sz; + + le_write16(&dest_h->index, 0); + le_write16(&dest_h->height, src_h.height); + le_write16(&dest_h->width, src_h.width); + le_write16(&dest_h->line_sz, dest_line_sz); + le_write16(&dest_h->bpp, dest_bpp); + le_write16(&dest_h->fixed_0, 0); /* seems to be fixed */ + le_write32(&dest_h->image_size, dest_line_sz * src_h.height); + le_write32(&dest_h->fixed_2c, 0x2c); /* seems to be fixed */ + le_write32(&dest_h->flag1, (dest_bpp == 8) ? 0x100 : 0); + le_write32(&dest_h->tr_color, 0xff00ff); /* magenta = transparent color */ + le_write32(&dest_h->flag2, 0x1); /* ? enable transparent mode ? */ + le_write32(&dest_h->size_2c, (dest_line_sz * src_h.height) + 0x2c); + + /* copy and revert order of BMP lines */ + ptr = (unsigned char*)dest_h; + ptr += (sizeof(*dest_h) + (dest_line_sz * (src_h.height - 1))); + + gbfseek(f, src_h.image_offset, SEEK_SET); + + if (src_h.bpp == 24) { + /* 24 bpp seems to be not supported, convert to 32 bpp */ + for (i = 0; i < src_h.height; i++) { + int j; + unsigned char* p = ptr; + + for (j = 0; j < src_h.width; j++) { + int color; + color = (int32_t)gbfgetint16(f) | (gbfgetc(f) << 16); + le_write32(p, color); + p += 4; + } + for (j = (src_h.width * src_h.bpp) / 8; j < src_line_sz; j++) { + gbfgetc(f); /* drop fill-in bytes */ + } + ptr -= dest_line_sz; + } + } else for (i = 0; i < src_h.height; i++) { + gbfread(ptr, 1, src_line_sz, f); + ptr -= dest_line_sz; + } + + if (src_h.used_colors > 0) { + ptr = (unsigned char*)dest_h; + ptr += (sizeof(*dest_h) + (src_h.height * src_line_sz)); + + for (i = 0; i < src_h.used_colors; i++) { + le_write32(ptr, color_table[i]); + ptr += 4; + } + } + + if (color_table) { + xfree(color_table); + } + gbfclose(f); + } + + /******************************************************************************* + * %%% global callbacks called by gpsbabel main process %%% * + *******************************************************************************/ + + static void + garmin_gpi_rd_init(const char* fname) + { + fin = gbfopen_le(fname, "rb", MYNAME); + rdata = new reader_data_t; + + read_header(); + + if ((codepage >= 1250) && (codepage <= 1257)) { + QString qCodecName = QString("windows-%1").arg(codepage); + cet_convert_init(CSTR(qCodecName), 1); + } else { + fatal(MYNAME ": Unsupported code page (%d). File is likely encrypted.\n", codepage); + } + + units = tolower(opt_units[0]); + if ((units != 'm') && (units != 's')) { + fatal(MYNAME ": Unknown units parameter (%c).\n", opt_units[0]); + } + } + + + static void + garmin_gpi_wr_init(const char* fname) + { + int i; + + if (gpi_timestamp != 0) { /* not the first gpi output session */ + time_t t = time(NULL); + if (t <= gpi_timestamp) { + gpi_timestamp++; /* don't create files with same timestamp */ + } else { + gpi_timestamp = t; + } + } else { + gpi_timestamp = gpsbabel_time; /* always ZERO during 'testo' */ + } + + fout = gbfopen_le(fname, "wb", MYNAME); + + short_h = mkshort_new_handle(); + + setshort_length(short_h, 1024); + setshort_badchars(short_h, "\r\n"); + setshort_mustupper(short_h, 0); + setshort_mustuniq(short_h, 1); + setshort_whitespace_ok(short_h, 1); + setshort_repeating_whitespace_ok(short_h, 0); + setshort_defname(short_h, "POI"); + + codepage = 0; + + for (i = 1250; i <= 1257; i++) { + if (QString("windows-%1").arg(i).compare(QString(opt_writecodec), Qt::CaseInsensitive) == 0) { + codepage = i; + break; + } + } + + if (! codepage) { + warning(MYNAME ": Unsupported character set (%s)!\n", opt_writecodec); + fatal(MYNAME ": Valid values are windows-1250 to windows-1257.\n"); + } + + cet_convert_init(opt_writecodec,1); + + units = tolower(opt_units[0]); + if ((units != 'm') && (units != 's')) { + fatal(MYNAME ": Unknown units parameter (%c).\n", opt_units[0]); + } + + alerts = (opt_alerts) ? 1 : 0; + + if (opt_speed) { + double scale; + alerts = 1; /* Force alerts to be enabled */ + if (units == 's') { + scale = MPH_TO_MPS(1); /* We need speed in meters per second */ + } else { + scale = KPH_TO_MPS(1); + } + parse_speed(opt_speed, &defspeed, scale, MYNAME); + } + + if (opt_proximity) { + double scale; + alerts = 1; /* Force alerts to be enabled */ + if (units == 's') { + scale = MILES_TO_METERS(1); /* We need proximity in meters */ + } else { + scale = 1000.0; /* one kilometer in meters */ + } + parse_distance(opt_proximity, &defproximity, scale, MYNAME); + } + wdata = wdata_alloc(); + } + + + static void + garmin_gpi_rd_deinit(void) + { + delete rdata; + gbfclose(fin); + } + + + static void + garmin_gpi_wr_deinit(void) + { + wdata_free(wdata); + mkshort_del_handle(&short_h); + gbfclose(fout); + + if ((opt_sleep) && (gpi_timestamp != 0)) { /* don't sleep during 'testo' */ + int sleep = atoi(opt_sleep); + if (sleep < 1) { + sleep = 1; + } + gpi_timestamp += sleep; + while (gpi_timestamp > time(NULL)) { + gb_sleep(100); + } + } + } + + + static void + garmin_gpi_read(void) + { + while (1) { + int tag = gbfgetint32(fin); + if (tag == 0xffff) { + return; + } + if (! read_tag("garmin_gpi_read", tag, NULL)) { + return; + } + }; + } + + + static void + garmin_gpi_write(void) + { + unsigned char* image; + int image_sz; + + if (strlen(opt_cat) == 0) { + fatal(MYNAME ": Can't write empty category!\n"); + } + + if (opt_hide_bitmap) { + image = NULL; + image_sz = 0; + } else if (opt_bitmap && *opt_bitmap) { + load_bitmap_from_file(opt_bitmap, &image, &image_sz); + } else { + image = gpi_bitmap; /* embedded GPSBabel icon in gpi format */ + image_sz = GPI_BITMAP_SIZE; + } + waypt_disp_all(enum_waypt_cb); + + wdata_check(wdata); + write_header(); + write_category(opt_cat, image, image_sz); + + gbfputint32(0xffff, fout); /* final tag */ + gbfputint32(0, fout); /* ? dummy size ? */ + + if (image != gpi_bitmap) { + xfree(image); + } + } + + /**************************************************************************/ + + ff_vecs_t garmin_gpi_vecs = { + ff_type_file, + { + (ff_cap)(ff_cap_read | ff_cap_write) /* waypoints */, + ff_cap_none /* tracks */, + ff_cap_none /* routes */ + }, + garmin_gpi_rd_init, + garmin_gpi_wr_init, + garmin_gpi_rd_deinit, + garmin_gpi_wr_deinit, + garmin_gpi_read, + garmin_gpi_write, + NULL, + garmin_gpi_args, + CET_CHARSET_MS_ANSI, 0 /* WIN-CP1252 */ + }; + + /**************************************************************************/ diff --cc nmea.cc index 000000000,6b221a313..7cc5fdc14 mode 000000,100644..100644 --- a/nmea.cc +++ b/nmea.cc @@@ -1,0 -1,1483 +1,1473 @@@ + /* + Read files containing selected NMEA 0183 sentences. + Based on information by Eino Uikkanenj + + Copyright (C) 2004-2006 Robert Lipe, robertlipe+source@gpsbabel.org + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA + + */ + + + #include "defs.h" + #include "cet_util.h" + #include "gbser.h" + #include "strptime.h" + #include "jeeps/gpsmath.h" + + #include + #include + #include + #include + #include + ++#include ++ + /********************************************************** + + ' 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 + ' $GPGGA - Global Positioning System Fix Data + ' $GPGGA,155537,6006.718,N,02426.290,E,1,05,2.4,50.5,M,19.7,M,,*79 + ' 2 123519 Fix taken at 12:35:19 UTC + ' 3,4 4807.038,N Latitude 48 deg 07.038' N + ' 5,6 01131.324,E Longitude 11 deg 31.324' E + ' 7 1 Fix quality: 0 = invalid + ' 1 = GPS fix + ' 2 = DGPS fix + ' 8 08 Number of satellites being tracked + ' 9 0.9 Horizontal dilution of position + ' 10,11 545.4,M Altitude, Metres, above mean sea level + ' 12,13 46.9,M Height of geoid (mean sea level) above WGS84 ellipsoid + ' 14 (empty field) time in seconds since last DGPS update + ' 15 (empty field) DGPS station ID number + + ' $GPWPL - waypoint location + ' $GPWPL,4917.16,N,12310.64,W,003*65 + ' 2,3 4917.16,N Latitude of waypoint + ' 4,5 12310.64,W Longitude of waypoint + ' 6 003 Waypoint ID + + ' $GPGLL - Geographic position, Latitude and Longitude + ' $GPGLL,4916.45,N,12311.12,W,225444,A + ' 2,3 4916.46,N Latitude 49 deg. 16.45 min. North + ' 4,5 12311.12,W Longitude 123 deg. 11.12 min. West + ' 6 225444 Fix taken at 22:54:44 UTC + ' 7 A Data valid + + ' $GPRMC - Recommended minimum specific GNSS Data + ' $GPRMC,085721.194,A,5917.7210,N,01103.9227,E,21.42,50.33,300504,,*07 + ' 2 085721 Fix taken at 08:57:21 UTC + ' 3 A Fix valid (this field reads V if fix is not valid) + ' 4,5 5917.7210,N Latitude 59 deg 17.7210' N + ' 6,7 01103.9227,E Longitude 11 deg 03.9227' E + ' 8 21.42 Speed over ground (knots) + ' 9 50.33 Course over ground (true) + ' 10 300504 Date 30/05-2004 + ' 11 Empty field Magnetic variation + + GSA - GPS DOP and active satellites + $GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39 + A Auto selection of 2D or 3D fix (M = manual) + 3 3D fix + 04,05... PRNs of satellites used for fix (space for 12) + 2.5 PDOP (dilution of precision) + 1.3 Horizontal dilution of precision (HDOP) + 2.1 Vertical dilution of precision (VDOP) + DOP is an indication of the effect of satellite geometry on + the accuracy of the fix. + + VTG - Track made good and ground speed + $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K + 054.7,T True track made good + 034.4,M Magnetic track made good + 005.5,N Ground speed, knots + 010.2,K Ground speed, Kilometers per hour + + WPL - waypoint location + $GPWPL,4917.16,N,12310.64,W,003*65 + 4917.16,N Latitude of waypoint + 12310.64,W Longitude of waypoint + 003 Waypoint ID + When a route is active, this sentence is sent once for each + waypoint in the route, in sequence. When all waypoints have + been reported, GPR00 is sent in the next data set. In any + group of sentences, only one WPL sentence, or an R00 + sentence, will be sent. + + + ' The optional checksum field consists of a "*" and two hex digits repre- + ' senting the exclusive OR of all characters between, but not including, + ' the "$" and "*". A checksum is required on some sentences. + + ****************************************/ + + /* + * An input file may have both GGA and GLL and RMC sentences for the exact + * same position fix. If we see a single GGA, start ignoring GLL's and RMC's. + * GLL's will also be ignored if RMC's are found and GGA's not found. + */ + + /* + Zmarties notes: + + In practice, all fields of the NMEA sentences should be treated as optional - + if the data is not available, then the field can be omitted (hence leading + to the output of two consecutive commas). + + An NMEA recording can start anywhere in the stream of data. It is therefore + necessary to discard sentences until sufficient data has been processed to + have all the necessary data to construct a waypoint. In practice, this means + discarding data until we have had the first sentence that provides the date. + (We could scan forwards in the stream of data to find the first date, and + then back apply it to all previous sentences, but that is probably more + complexity that is necessary - the lost of one waypoint at the start of the + stream can normally be tolerated.) + + If a sentence is received without a checksum, but previous sentences have + had checksums, it is best to discard that sentence. In practice, the only + time I have seen this is when the recording stops suddenly, where the last + sentence is truncated - and missing part of the line, including the checksum. + */ + + typedef enum { + gp_unknown = 0, + gpgga, + gplgll, + gprmc + } preferred_posn_type; + + static enum { + rm_unknown = 0, + rm_serial, + rm_file + } read_mode; + + static gbfile* file_in, *file_out; + static route_head* trk_head; + static short_handle mkshort_handle; + static preferred_posn_type posn_type; + static struct tm tm; + static Waypoint* curr_waypt; + static Waypoint* last_waypt; + static void* gbser_handle; + static const char* posn_fname; + static queue pcmpt_head; + + static int without_date; /* number of created trackpoints without a valid date */ + static struct tm opt_tm; /* converted "date" parameter */ + + #define MYNAME "nmea" + + static char* opt_gprmc; + static char* opt_gpgga; + static char* opt_gpvtg; + static char* opt_gpgsa; + static char* snlenopt; + static char* optdate; + static char* getposnarg; + static char* opt_sleep; + static char* opt_baud; + static char* opt_append; + static char* opt_gisteq; + static char* opt_ignorefix; + + static long sleepus; + static int getposn; + static int append_output; + static int amod_waypoint; + + static time_t last_time; + static double last_read_time; /* Last timestamp of GGA or PRMC */ + static int datum; + static int had_checksum; + + static Waypoint* nmea_rd_posn(posn_status*); + static void nmea_rd_posn_init(const char* fname); + + arglist_t nmea_args[] = { + {"snlen", &snlenopt, "Max length of waypoint name to write", "6", ARGTYPE_INT, "1", "64" }, + {"gprmc", &opt_gprmc, "Read/write GPRMC sentences", "1", ARGTYPE_BOOL, ARG_NOMINMAX }, + {"gpgga", &opt_gpgga, "Read/write GPGGA sentences", "1", ARGTYPE_BOOL, ARG_NOMINMAX }, + {"gpvtg", &opt_gpvtg, "Read/write GPVTG sentences", "1", ARGTYPE_BOOL, ARG_NOMINMAX }, + {"gpgsa", &opt_gpgsa, "Read/write GPGSA sentences", "1", ARGTYPE_BOOL, ARG_NOMINMAX }, + {"date", &optdate, "Complete date-free tracks with given date (YYYYMMDD).", NULL, ARGTYPE_INT, ARG_NOMINMAX }, + { + "get_posn", &getposnarg, "Return current position as a waypoint", + NULL, ARGTYPE_BOOL, ARG_NOMINMAX + }, + {"pause", &opt_sleep, "Decimal seconds to pause between groups of strings", NULL, ARGTYPE_INT, ARG_NOMINMAX }, + {"append_positioning", &opt_append, "Append realtime positioning data to the output file instead of truncating", "0", ARGTYPE_BOOL, ARG_NOMINMAX }, + {"baud", &opt_baud, "Speed in bits per second of serial port (baud=4800)", NULL, ARGTYPE_INT, ARG_NOMINMAX }, + {"gisteq", &opt_gisteq, "Write tracks for Gisteq Phototracker", "0", ARGTYPE_BOOL, ARG_NOMINMAX }, + {"ignore_fix", &opt_ignorefix, "Accept position fixes in gpgga marked invalid", "0", ARGTYPE_BOOL, ARG_NOMINMAX }, + ARG_TERMINATOR + }; + + #define CHECK_BOOL(a) if (a && (*a == '0')) a = NULL + + /* + * Slightly different than the Magellan checksum fn. + */ + int + nmea_cksum(const char* const buf) + { + int x = 0 ; + const char* p; + + for (p = buf; *p; p++) { + x ^= *p; + } + return x; + } + + static void + nmea_add_wpt(Waypoint* wpt, route_head* trk) + { + if (datum != DATUM_WGS84) { + double lat, lon, alt; + GPS_Math_Known_Datum_To_WGS84_M( + wpt->latitude, wpt->longitude, 0, + &lat, &lon, &alt, datum); + wpt->latitude = lat; + wpt->longitude = lon; + } + if (trk != NULL) { + track_add_wpt(trk, wpt); + } else { + waypt_add(wpt); + } + } + + static void + nmea_release_wpt(Waypoint* wpt) + { + if (wpt && ((wpt->Q.next == NULL) || (wpt->Q.next == &wpt->Q))) { + /* This waypoint isn't queued. + Release it, because we don't have any reference to this + waypoint (! memory leak !) */ + delete wpt; + } + } + + static void + nmea_rd_init(const char* fname) + { + curr_waypt = NULL; + last_waypt = NULL; + last_time = -1; + datum = DATUM_WGS84; + had_checksum = 0; + + CHECK_BOOL(opt_gprmc); + CHECK_BOOL(opt_gpgga); + CHECK_BOOL(opt_gpvtg); + CHECK_BOOL(opt_gpgsa); + CHECK_BOOL(opt_gisteq); + + QUEUE_INIT(&pcmpt_head); + + if (getposnarg) { + getposn = 1; + } + + /* A special case hack that gets our current position and returns + * it as one waypoint. + */ + if (getposn) { + Waypoint* wpt; + posn_status st; + nmea_rd_posn_init(fname); + wpt = nmea_rd_posn(&st); + if (!wpt) { + return; + } + wpt->shortname = "Position"; + nmea_add_wpt(wpt, NULL); + return; + } + + read_mode = rm_file; + file_in = gbfopen(fname, "rb", MYNAME); + } + + static void + nmea_rd_deinit(void) + { + switch (read_mode) { + case rm_serial: + gbser_deinit(gbser_handle); + break; + case rm_file: + gbfclose(file_in); + file_in = NULL; + break; + default: + fatal("nmea_rd_deinit: illegal read_mode.\n"); + break; + } + } + + static void + nmea_wr_init(const char* portname) + { + CHECK_BOOL(opt_gprmc); + CHECK_BOOL(opt_gpgga); + CHECK_BOOL(opt_gpvtg); + CHECK_BOOL(opt_gpgsa); + CHECK_BOOL(opt_gisteq); + + append_output = atoi(opt_append); + + file_out = gbfopen(portname, append_output ? "a+" : "w+", MYNAME); + + sleepus = -1; + if (opt_sleep) { + if (*opt_sleep) { + sleepus = 1e6 * atof(opt_sleep); + } else { + sleepus = -1; + } + } + + mkshort_handle = mkshort_new_handle(); + setshort_length(mkshort_handle, atoi(snlenopt)); + + if (opt_gisteq) { + opt_gpgga = NULL; + opt_gpvtg = NULL; + opt_gpgsa = NULL; + } + } + + static void + nmea_wr_deinit(void) + { + gbfclose(file_out); + mkshort_del_handle(&mkshort_handle); + } + + static void + nmea_set_waypoint_time(Waypoint* wpt, struct tm* time, double fsec) + { + if (time->tm_year == 0) { + wpt->SetCreationTime(((((time_t)time->tm_hour * 60) + time->tm_min) * 60) + time->tm_sec, lround(1000.0 * fsec)); + if (wpt->wpt_flags.fmt_use == 0) { + wpt->wpt_flags.fmt_use = 1; + without_date++; + } + } else { + wpt->SetCreationTime(mkgmtime(time), lround(1000.0 * fsec)); + if (wpt->wpt_flags.fmt_use != 0) { + wpt->wpt_flags.fmt_use = 0; + without_date--; + } + } + } + + static void + gpgll_parse(char* ibuf) + { - double latdeg, lngdeg; - double fsec; - char lngdir, latdir; - double hmsd; - int hms; - char valid = 0; - Waypoint* waypt; - + if (trk_head == NULL) { + trk_head = route_head_alloc(); + track_add_head(trk_head); + } + - sscanf(ibuf,"$%*2cGLL,%lf,%c,%lf,%c,%lf,%c,", - &latdeg,&latdir, - &lngdeg,&lngdir, - &hmsd,&valid); ++ QStringList fields = QString(ibuf).split(",", QString::KeepEmptyParts); + - if (valid != 'A') { ++ double latdeg = 0; ++ if (fields.size() > 1) fields[1].toDouble(); ++ QChar latdir = 'N'; ++ if (fields.size() > 2) latdir = fields[2][0]; ++ double lngdeg = 0; ++ if (fields.size() > 3) lngdeg = fields[3].toDouble(); ++ QChar lngdir = 'E'; ++ if (fields.size() > 4) lngdir = fields[4][0]; ++ double hmsd = 0; ++ if (fields.size() > 5) hmsd = fields[5].toDouble(); ++ bool valid = false; ++ if (fields.size() > 6) valid = fields[6].startsWith('A'); ++ ++ if (!valid) { + return; + } + - hms = (int) hmsd; ++ int hms = (int) hmsd; + last_read_time = hms; - fsec = hmsd - hms; ++ double fsec = hmsd - hms; + + tm.tm_sec = hms % 100; + hms = hms / 100; + tm.tm_min = hms % 100; + hms = hms / 100; + tm.tm_hour = hms % 100; + - waypt = new Waypoint; ++ Waypoint* waypt = new Waypoint; + + nmea_set_waypoint_time(waypt, &tm, fsec); + + if (latdir == 'S') { + latdeg = -latdeg; + } + waypt->latitude = ddmm2degrees(latdeg); + + if (lngdir == 'W') { + lngdeg = -lngdeg; + } + waypt->longitude = ddmm2degrees(lngdeg); + + nmea_release_wpt(curr_waypt); + curr_waypt = waypt; + } + + static void + gpgga_parse(char* ibuf) + { - double latdeg, lngdeg; - char lngdir, latdir; - double hms; - double alt; - int fix = fix_unknown; - int nsats = 0; - double hdop; - char altunits; - double geoidheight; - char geoidheightunits; - Waypoint* waypt; - double fsec; - + if (trk_head == NULL) { + trk_head = route_head_alloc(); + track_add_head(trk_head); + } + - sscanf(ibuf,"$%*2cGGA,%lf,%lf,%c,%lf,%c,%d,%d,%lf,%lf,%c,%lf,%c", - &hms, &latdeg,&latdir, - &lngdeg,&lngdir, - &fix,&nsats,&hdop,&alt,&altunits,&geoidheight,&geoidheightunits); ++ QStringList fields = QString(ibuf).split(",", QString::KeepEmptyParts); ++ double hms = 0; ++ if (fields.size() > 1) hms = fields[1].toDouble(); ++ double latdeg = 0; ++ if (fields.size() > 2) latdeg = fields[2].toDouble(); ++ QChar latdir = 'N'; ++ if (fields.size() > 3) latdir = fields[3][0]; ++ double lngdeg = 0; ++ if (fields.size() > 4) lngdeg = fields[4].toDouble(); ++ QChar lngdir = 'W'; ++ if (fields.size() > 5) lngdir = fields[5][0]; ++ int fix = fix_unknown; ++ if (fields.size() > 6) fix = fields[6].toInt(); ++ int nsats = 0; ++ if (fields.size() > 7) nsats = fields[7].toInt(); ++ double hdop = 0; ++ if (fields.size() > 8) hdop = fields[8].toDouble(); ++ double alt = unknown_alt; ++ if (fields.size() > 9) alt = fields[9].toDouble(); ++ QChar altunits; ++ if (fields.size() > 10) altunits = fields[10][0]; ++ double geoidheight = unknown_alt; ++ if (fields.size() > 11) geoidheight = fields[11].toDouble(); ++ QChar geoidheightunits = 'M'; ++ if (fields.size() > 12) geoidheightunits = fields[12][0]; + + /* + * In serial mode, allow the fix with an invalid position through + * as serial units will often spit a remembered position up and + * that is more comfortable than nothing at all... + */ + CHECK_BOOL(opt_ignorefix); + if ((fix <= 0) && (read_mode != rm_serial) && (!opt_ignorefix)) { + return; + } + + last_read_time = hms; - fsec = hms - (int)hms; ++ double fsec = hms - (int)hms; + + tm.tm_sec = (long) hms % 100; + hms = hms / 100; + tm.tm_min = (long) hms % 100; + hms = hms / 100; + tm.tm_hour = (long) hms % 100; + - waypt = new Waypoint; ++ Waypoint* waypt = new Waypoint; + + nmea_set_waypoint_time(waypt, &tm, fsec); + + if (latdir == 'S') { + latdeg = -latdeg; + } + waypt->latitude = ddmm2degrees(latdeg); + + if (lngdir == 'W') { + lngdeg = -lngdeg; + } + waypt->longitude = ddmm2degrees(lngdeg); + + waypt->altitude = alt; + + WAYPT_SET(waypt, geoidheight, geoidheight); + + waypt->sat = nsats; + + waypt->hdop = hdop; + + switch (fix) { + case 0: + waypt->fix = fix_none; + break; + case 1: + waypt->fix = (nsats>3)?(fix_3d):(fix_2d); + break; + case 2: + waypt->fix = fix_dgps; + break; + case 3: + waypt->fix = fix_pps; + break; + } + + nmea_release_wpt(curr_waypt); + curr_waypt = waypt; + } + + static void + gprmc_parse(char* ibuf) + { - double latdeg, lngdeg; - char lngdir, latdir; - double hms; - char fix; - unsigned int dmy; - double speed,course; - Waypoint* waypt; - double fsec; - char* dmybuf; - int i; - + if (trk_head == NULL) { + trk_head = route_head_alloc(); + track_add_head(trk_head); + } + - /* - * Read everything except the dmy, in case lngdeg - * and lngdir are missing. - */ - sscanf(ibuf,"$%*2cRMC,%lf,%c,%lf,%c,%lf,%c,%lf,%lf", - &hms, &fix, &latdeg, &latdir, - &lngdeg, &lngdir, - &speed, &course); ++ QStringList fields = QString(ibuf).split(",", QString::KeepEmptyParts); ++ double hms = 0; ++ if (fields.size() > 1) hms = fields[1].toDouble(); ++ QChar fix = 'V'; // V == "Invalid" ++ if (fields.size() > 2) fix = fields[2][0]; ++ double latdeg = 0; ++ if (fields.size() > 3) latdeg = fields[3].toDouble(); ++ QChar latdir = 'N'; ++ if (fields.size() > 4) latdir = fields[4][0]; ++ double lngdeg = 0; ++ if (fields.size() > 5) lngdeg = fields[5].toDouble(); ++ QChar lngdir = 'W'; ++ if (fields.size() > 6) lngdir = fields[6][0]; ++ double speed = 0; ++ if (fields.size() > 7) speed = fields[7].toDouble(); ++ double course = 0; ++ if (fields.size() > 8) course = fields[8].toDouble(); ++ int dmy = 0; ++ if (fields.size() > 9) dmy = fields[9].toDouble(); + + if (fix != 'A') { + /* ignore this fix - it is invalid */ + return; + } + - /* Skip past nine commas in ibuf to reach the dmy value */ - for (dmybuf=ibuf,i=0; i<9; i++) { - dmybuf= strchr(dmybuf, ','); - if (dmybuf==NULL) { - /* If we run out of commas, the sentence is invalid. */ - return; - } - dmybuf++; - } - - /* Now read dmy from the correct position */ - sscanf(dmybuf,"%u", &dmy); - + last_read_time = hms; - fsec = hms - (int)hms; ++ double fsec = hms - (int)hms; + + tm.tm_sec = (long) hms % 100; + hms = hms / 100; + tm.tm_min = (long) hms % 100; + hms = hms / 100; + tm.tm_hour = (long) hms % 100; + + tm.tm_year = dmy % 100 + 100; + dmy = dmy / 100; + tm.tm_mon = dmy % 100 - 1; + dmy = dmy / 100; + tm.tm_mday = dmy; + + if (posn_type == gpgga) { + /* capture useful data update and exit */ + if (curr_waypt) { + if (! WAYPT_HAS(curr_waypt, speed)) { + WAYPT_SET(curr_waypt, speed, KNOTS_TO_MPS(speed)); + } + if (! WAYPT_HAS(curr_waypt, course)) { + WAYPT_SET(curr_waypt, course, course); + } + /* The change of date wasn't recorded when + * going from 235959 to 000000. */ + nmea_set_waypoint_time(curr_waypt, &tm, fsec); + } + /* This point is both a waypoint and a trackpoint. */ + if (amod_waypoint) { + waypt_add(new Waypoint(*curr_waypt)); + amod_waypoint = 0; + } + return; + } + - waypt = new Waypoint; ++ Waypoint* waypt = new Waypoint; + + WAYPT_SET(waypt, speed, KNOTS_TO_MPS(speed)); - + WAYPT_SET(waypt, course, course); + + nmea_set_waypoint_time(waypt, &tm, fsec); + + if (latdir == 'S') { + latdeg = -latdeg; + } + waypt->latitude = ddmm2degrees(latdeg); + + if (lngdir == 'W') { + lngdeg = -lngdeg; + } + waypt->longitude = ddmm2degrees(lngdeg); + + nmea_release_wpt(curr_waypt); + curr_waypt = waypt; + + /* This point is both a waypoint and a trackpoint. */ + if (amod_waypoint) { + waypt_add(new Waypoint(*waypt)); + amod_waypoint = 0; + } + } + + static void + gpwpl_parse(char* ibuf) + { - Waypoint* waypt; + double latdeg, lngdeg; + char latdir, lngdir; + char sname[99]; + + sscanf(ibuf,"$%*2cWPL,%lf,%c,%lf,%c,%98[^*]", + &latdeg,&latdir, + &lngdeg,&lngdir, + sname); + - waypt = new Waypoint; ++ Waypoint* waypt = new Waypoint; + + if (latdir == 'S') { + latdeg = -latdeg; + } + waypt->latitude = ddmm2degrees(latdeg); + if (lngdir == 'W') { + lngdeg = -lngdeg; + } + waypt->longitude = ddmm2degrees(lngdeg); + + waypt->shortname = sname; + + curr_waypt = NULL; /* waypoints won't be updated with GPS fixes */ + nmea_add_wpt(waypt, NULL); + } + + static void + gpzda_parse(char* ibuf) + { + double hms; + int dd, mm, yy, lclhrs, lclmins; + + sscanf(ibuf,"$%*2cZDA,%lf,%d,%d,%d,%d,%d", + &hms, &dd, &mm, &yy, &lclhrs, &lclmins); + tm.tm_sec = (int) hms % 100; + tm.tm_min = (((int) hms - tm.tm_sec) / 100) % 100; + tm.tm_hour = (int) hms / 10000; + tm.tm_mday = dd; + tm.tm_mon = mm - 1; + tm.tm_year = yy - 1900; ++ // FIXME: why do we do all this and then do nothing with the result? ++ // This can't have worked. + } + + static void + gpgsa_parse(char* ibuf) + { + char fixauto; + char fix; + int prn[12] = {0}; + int scn,cnt; + float pdop=0,hdop=0,vdop=0; + char* tok=0; + + memset(prn,0xff,sizeof(prn)); + + scn = sscanf(ibuf,"$%*2cGSA,%c,%c,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d", + &fixauto, &fix, + &prn[0],&prn[1],&prn[2],&prn[3],&prn[4],&prn[5], + &prn[6],&prn[7],&prn[8],&prn[9],&prn[10],&prn[11]); + if (scn < 4) { + warning(MYNAME ": Short GSA sentence."); + } + /* + sscanf has scanned all the leftmost elements + we'll rescan by skipping 15 commas to the dops + */ + tok = ibuf; + for (cnt=0; (tok)&&(cnt<15); cnt++) { + tok = strchr(tok,','); + if (!tok) { + break; + } + tok++; + } + if (tok) { + sscanf(tok,"%f,%f,%f",&pdop,&hdop,&vdop); + } + + + if (curr_waypt) { + + if (curr_waypt->fix!=fix_dgps) { + if (fix=='3') { + curr_waypt->fix=fix_3d; + } else if (fix=='2') { + curr_waypt->fix=fix_2d; + } + } + + curr_waypt->pdop = pdop; + curr_waypt->hdop = hdop; + curr_waypt->vdop = vdop; + + if (curr_waypt->sat <= 0) { + for (cnt=0; cnt<12; cnt++) { + curr_waypt->sat += (prn[cnt]>0)?(1):(0); + } + } + } + + } + + static void + gpvtg_parse(char* ibuf) + { - float course; - char ct; - float magcourse; - char cm; - double speed_n; - char cn; - double speed_k; - char ck; - - sscanf(ibuf,"$%*2cVTG,%f,%c,%f,%c,%lf,%c,%lf,%c", - &course,&ct,&magcourse,&cm,&speed_n,&cn,&speed_k,&ck); ++ QStringList fields = QString(ibuf).split(",", QString::KeepEmptyParts); ++ double course = 0; ++ if (fields.size() > 1) course = fields[1].toDouble(); ++ double speed_n = 0; ++ if (fields.size() > 5) speed_n = fields[5].toDouble(); ++ double speed_k = 0; ++ if (fields.size() > 7) speed_k = fields[7].toDouble(); + + if (curr_waypt) { + WAYPT_SET(curr_waypt, course, course); - - if (speed_k>0) ++ if (speed_k > 0) { + WAYPT_SET(curr_waypt, speed, KPH_TO_MPS(speed_k)) - else { - WAYPT_SET(curr_waypt, speed, KNOTS_TO_MPS(speed_n)); - } - ++ } else { ++ WAYPT_SET(curr_waypt, speed, KNOTS_TO_MPS(speed_n)); ++ } + } + + } + + /* + * AVMAP EKP-IV Tracks - a proprietary (and very weird) extended NMEA. + * https://sourceforge.net/tracker/?func=detail&atid=489478&aid=1640814&group_id=58972 + */ + static + double pcmpt_deg(int d) + { - int deg; - double minutes; - - deg = d / 100000; - minutes = (((d / 100000.0) - deg) * 100) / 60.0; ++ int deg = d / 100000; ++ double minutes = (((d / 100000.0) - deg) * 100) / 60.0; + return (double) deg + minutes; + } + + void + pcmpt_parse(char* ibuf) + { + int i, j1, j2, j3, j4, j5, j6; + int lat, lon; + char altflag, u1, u2; + float alt, f1, f2; + char coords[20] = {0}; + int dmy, hms; + + dmy = hms = 0; + + sscanf(ibuf,"$PCMPT,%d,%d,%d,%c,%f,%d,%19[^,],%d,%f,%d,%f,%c,%d,%c,%d", + &j1, &j2, &j3, &altflag, &alt, &j4, (char*) &coords, + &j5, &f1, &j6, &f2, &u1, &dmy, &u2, &hms); + + if (altflag == 'D' && curr_waypt && alt > 0) { + curr_waypt->altitude = alt /*+ 500*/; + return; + } + + /* + * There are a couple of different second line records, but we + * don't care about them. + */ + if (j2 != 1) { + return; + } + + sscanf(coords, "%d%n", &lat, &i); + if (coords[i] == 'S') { + lat = -lat; + } + sscanf(coords + i + 1, "%d%n", &lon, &i); + if (coords[i] == 'W') { + lon= -lon; + } + + if (lat || lon) { + curr_waypt = new Waypoint; + curr_waypt->longitude = pcmpt_deg(lon); + curr_waypt->latitude = pcmpt_deg(lat); + + tm.tm_sec = (long) hms % 100; + hms = hms / 100; + tm.tm_min = (long) hms % 100; + hms = hms / 100; + tm.tm_hour = (long) hms % 100; + + tm.tm_year = dmy % 10000 - 1900; + dmy = dmy / 10000; + tm.tm_mon = dmy % 100 - 1; + dmy = dmy / 100; + tm.tm_mday = dmy; + nmea_set_waypoint_time(curr_waypt, &tm, 0); + ENQUEUE_HEAD(&pcmpt_head, &curr_waypt->Q); + } else { + queue* elem, *tmp; + route_head* trk_head; + + if (QUEUE_EMPTY(&pcmpt_head)) { + return; + } + + /* + * Since we oh-so-cleverly inserted points at the head, + * we can rip through the queue forward now to get our + ` * handy-dandy reversing effect. + */ + trk_head = route_head_alloc(); + track_add_head(trk_head); + QUEUE_FOR_EACH(&pcmpt_head, elem, tmp) { + Waypoint* wpt = (Waypoint*) dequeue(elem); + nmea_add_wpt(wpt, trk_head); + } + } + } + + static void + nmea_fix_timestamps(route_head* track) + { + if ((trk_head == NULL) || (without_date == 0)) { + return; + } + + if (tm.tm_year == 0) { + queue* elem, *temp; + Waypoint* prev = NULL; + time_t delta_tm; + + if (optdate == NULL) { + warning(MYNAME ": No date found within track (all points dropped)!\n"); + warning(MYNAME ": Please use option \"date\" to preset a valid date for thoose tracks.\n"); + track_del_head(track); + return; + } + delta_tm = mkgmtime(&opt_tm); + + QUEUE_FOR_EACH(&track->waypoint_list, elem, temp) { + Waypoint* wpt = (Waypoint*)elem; + + wpt->creation_time += delta_tm; + if ((prev != NULL) && (prev->creation_time > wpt->creation_time)) { /* go over midnight ? */ + delta_tm += SECONDS_PER_DAY; + wpt->creation_time += SECONDS_PER_DAY; + } + prev = wpt; + } + } else { + time_t prev; + queue* elem; + + tm.tm_hour = 23; /* last date found */ + tm.tm_min = 59; + tm.tm_sec = 59; + + prev = mkgmtime(&tm); + + /* go backward through the track and complete timestamps */ + + for (elem = QUEUE_LAST(&track->waypoint_list); elem != &track->waypoint_list; elem=elem->prev) { + Waypoint* wpt = (Waypoint*)elem; + + if (wpt->wpt_flags.fmt_use != 0) { + time_t dt; + + wpt->wpt_flags.fmt_use = 0; /* reset flag */ + + dt = (prev / SECONDS_PER_DAY) * SECONDS_PER_DAY; + wpt->creation_time += dt; + if (wpt->creation_time.toTime_t() > prev) { + wpt->creation_time+=SECONDS_PER_DAY; + } + } + prev = wpt->GetCreationTime().toTime_t(); + } + } + } + + static int + notalkerid_strmatch(const char * s1, const char *sentenceFormatterMnemonicCode) + { + /* + * compare leading start of parametric sentence character ('$'), sentence address field, and trailing comma + * to the desired sentence formatter mneumonic code (the 3rd-5th characters of the sentence address field). + * The talker identifier mneumonic (the 1st-2nd characters of the sentence address field) + * is likely "GP" for Global Posilioning System (GPS) + * but other talkers like "IN" for Integrated Navigation can emit relevant sentences, + * so we ignore the talker identifier mneumonic. + */ + return strncmp(s1,"$",1) || strncmp(s1+3,sentenceFormatterMnemonicCode,3) || strncmp(s1+6,",",1); + } + + void + nmea_parse_one_line(char* ibuf) + { + char* ck; + int ckval, ckcmp; + char* tbuf = lrtrim(ibuf); + + /* + * GISTEQ PhotoTracker (stupidly) puts a bogus field in front + * of the line. Look for it and toss it. + */ + if (0 == strncmp(tbuf, "---,", 4)) { + tbuf += 4; + } + + if (*tbuf != '$') { + return; + } + + ck = strrchr(tbuf, '*'); + if (ck != NULL) { + *ck = '\0'; + ckval = nmea_cksum(&tbuf[1]); + *ck = '*'; + ck++; + sscanf(ck, "%2X", &ckcmp); + if (ckval != ckcmp) { + #if 0 + printf("ckval %X, %X, %s\n", ckval, ckcmp, ck); + printf("NMEA %s\n", tbuf); + #endif + return; + } + + had_checksum = 1; + } else if (had_checksum) { + /* we have had a checksum on all previous sentences, but not on this + one, which probably indicates this line is truncated */ + had_checksum = 0; + return; + } + + if (strstr(tbuf+1,"$")!=NULL) { + /* If line has more than one $, there is probably an error in it. */ + return; + } + + /* @@@ zmarties: The parse routines all assume all fields are present, but + the NMEA format allows any field to be missed out if there is no data + for that field. Rather than change all the parse routines, we first + substitute a default value of zero for any missing field. + */ + if (strstr(tbuf, ",,")) { + tbuf = gstrsub(tbuf, ",,", ",0,"); + } + + if (0 == notalkerid_strmatch(tbuf, "WPL")) { + gpwpl_parse(tbuf); + } else if (opt_gpgga && (0 == notalkerid_strmatch(tbuf, "GGA"))) { + posn_type = gpgga; + gpgga_parse(tbuf); + } else if (opt_gprmc && (0 == notalkerid_strmatch(tbuf, "RMC"))) { + if (posn_type != gpgga) { + posn_type = gprmc; + } + /* + * Always call gprmc_parse() because like GPZDA + * it contains the full date. + */ + gprmc_parse(tbuf); + } else if (0 == notalkerid_strmatch(tbuf, "GLL")) { + if ((posn_type != gpgga) && (posn_type != gprmc)) { + gpgll_parse(tbuf); + } + } else if (0 == notalkerid_strmatch(tbuf, "ZDA")) { + gpzda_parse(tbuf); + } else if (0 == strncmp(tbuf, "$PCMPT,", 7)) { + pcmpt_parse(tbuf); + } else if (opt_gpvtg && (0 == notalkerid_strmatch(tbuf, "VTG"))) { + gpvtg_parse(tbuf); /* speed and course */ + } else if (opt_gpgsa && (0 == notalkerid_strmatch(tbuf, "GSA"))) { + gpgsa_parse(tbuf); /* GPS fix */ + } else if (0 == strncmp(tbuf, "$ADPMB,5,0", 10)) { + amod_waypoint = 1; + } + + if (tbuf != ibuf) { + /* clear up the dynamic buffer we used because substition was required */ + xfree(tbuf); + } + } + + static void + nmea_read(void) + { + char* ibuf; + char* ck; + double lt = -1; + int line = -1; + + posn_type = gp_unknown; + trk_head = NULL; + without_date = 0; + memset(&tm, 0, sizeof(tm)); + opt_tm = tm; + + /* This was done in rd_init() */ + if (getposn) { + return; + } + + if (optdate) { + memset(&opt_tm, 0, sizeof(opt_tm)); + + ck = (char*)strptime(optdate, "%Y%m%d", &opt_tm); + if ((ck == NULL) || (*ck != '\0') || (strlen(optdate) != 8)) { + fatal(MYNAME ": Invalid date \"%s\"!\n", optdate); + } else if (opt_tm.tm_year < 70) { + fatal(MYNAME ": Date \"%s\" is out of range (have to be 19700101 or later)!\n", optdate); + } + } + + curr_waypt = NULL; + + while ((ibuf = gbfgetstr(file_in))) { + char* sdatum, *cx; + + line++; + + if ((line == 0) & file_in->unicode) { + cet_convert_init(CET_CHARSET_UTF8, 1); + } + + if ((line == 0) && (case_ignore_strncmp(ibuf, "@SonyGPS/ver", 12) == 0)) { + /* special hack for Sony GPS-CS1 files: + they are fully (?) nmea compatible, but come with a header line like + "@Sonygps/ver1.0/wgs-84". */ + /* The Sony GPS-CS3KA extends that line even further + so we now look for the second field to be / + delimited. + @Sonygps/ver1.0/wgs-84/gps-cs3.0 + */ + + /* Check the GPS datum */ + cx = strchr(&ibuf[12], '/'); + if (cx != NULL) { + char* edatum; + sdatum = cx + 1; + edatum = strchr(sdatum, '/'); + if (edatum) { + *edatum = 0; + } + datum = GPS_Lookup_Datum_Index(sdatum); + if (datum < 0) { + fatal(MYNAME "/SonyGPS: Unsupported datum \"%s\" in source data!\n", sdatum); + } + } + continue; + } + + nmea_parse_one_line(ibuf); + if (lt != last_read_time && curr_waypt && trk_head) { + if (curr_waypt != last_waypt) { + nmea_add_wpt(curr_waypt, trk_head); + last_waypt = curr_waypt; + } + lt = last_read_time; + } + } + + /* try to complete date-less trackpoints */ + nmea_fix_timestamps(trk_head); + } + + void + nmea_rd_posn_init(const char* fname) + { + if ((gbser_handle = gbser_init(fname)) != NULL) { + read_mode = rm_serial; + gbser_set_speed(gbser_handle, 4800); + } else { + fatal(MYNAME ": Could not open '%s' for position tracking.\n", fname); + } + + gbser_flush(gbser_handle); + + if (opt_baud) { + if (!gbser_set_speed(gbser_handle, atoi(opt_baud))) { + fatal(MYNAME ": Unable to set baud rate %s\n", opt_baud); + } + } + posn_fname = fname; + } + + static void + safe_print(int cnt, const char* b) + { + int i; + for (i = 0; i < cnt; i++) { + char c = isprint(b[i]) ? b[i] : '.'; + fputc(c, stderr); + } + } + + static void reset_sirf_to_nmea(int br); + + static + int hunt_sirf(void) + { + /* Try to place the common BR's first to speed searching */ + static int br[] = {38400, 9600, 57600, 115200, 19200, 4800, -1}; + static int* brp = &br[0]; + char ibuf[1024]; + + for (brp = br; *brp > 0; brp++) { + int rv; + if (global_opts.debug_level > 1) { + fprintf(stderr, "Trying %d\n", *brp); + } + + /* + * Cycle our port's data speed and spray the "change to NMEA + * mode to the device. + */ + gbser_set_speed(gbser_handle, *brp); + reset_sirf_to_nmea(*brp); + + rv = gbser_read_line(gbser_handle, ibuf, sizeof(ibuf), + 1000, 0x0a, 0x0d); + /* + * If we didn't get a read error but did get a string that + * started with a dollar sign, we're probably in NMEA mode + * now. + */ + if ((rv > -1) && (strlen(ibuf) > 0) && ibuf[0] == '$') { + return 1; + } + + /* + * If nothing was received, it's not a sirf part. Fast exit. + */ + if (rv < 0) { + return 0; + } + } + return 0; + } + + static Waypoint* + nmea_rd_posn(posn_status* posn_status) + { + char ibuf[1024]; + static double lt = -1; + int i; + int am_sirf = 0; + + /* + * Read a handful of sentences, collecting the best info we + * can. If the timestamp changes (indicating the sequence is + * about to restart and thus the one we're collecting isn't going + * to get any better than we now have) hand that back to the caller. + */ + + for (i = 0; i < 10; i++) { + int rv; + ibuf[0] = 0; + rv = gbser_read_line(gbser_handle, ibuf, sizeof(ibuf), 2000, 0x0a, 0x0d); + if (global_opts.debug_level > 1) { + safe_print(strlen(ibuf), ibuf); + } + if (rv < 0) { + if (am_sirf == 0) { + if (global_opts.debug_level > 1) { + warning(MYNAME ": Attempting sirf mode.\n"); + } + /* This is tacky, we have to change speed + * to 9600bps to tell it to speak NMEA at + * 4800. + */ + am_sirf = hunt_sirf(); + if (am_sirf) { + i = 0; + continue; + } + } + fatal(MYNAME ": No data received on %s.\n", posn_fname); + } + nmea_parse_one_line(ibuf); + if (lt != last_read_time) { + if (last_read_time) { + Waypoint* w = curr_waypt; + + lt = last_read_time; + curr_waypt = NULL; + + return w; + } + } + } + return NULL; + } + + static void + nmea_wayptpr(const Waypoint* wpt) + { + char obuf[200]; + double lat,lon; + QString s; + int cksum; + + lat = degrees2ddmm(wpt->latitude); + lon = degrees2ddmm(wpt->longitude); + if (global_opts.synthesize_shortnames) { + s = mkshort_from_wpt(mkshort_handle, wpt); + } else { + s = mkshort(mkshort_handle, wpt->shortname); + } + + snprintf(obuf, sizeof(obuf), "GPWPL,%08.3f,%c,%09.3f,%c,%s", + fabs(lat), lat < 0 ? 'S' : 'N', + fabs(lon), lon < 0 ? 'W' : 'E', CSTRc(s) + + ); + cksum = nmea_cksum(obuf); + gbfprintf(file_out, "$%s*%02X\n", obuf, cksum); + if (sleepus >= 0) { + gbfflush(file_out); + gb_sleep(sleepus); + } + } + + void + nmea_track_init(const route_head*) + { + last_time = -1; + } + + void + nmea_trackpt_pr(const Waypoint* wpt) + { + char obuf[200]; + char fix='0'; + double lat,lon; + int cksum; + struct tm* tm; + time_t hms; + time_t ymd; + + if (opt_sleep) { + gbfflush(file_out); + if (last_time > 0) { + if (sleepus >= 0) { + gb_sleep(sleepus); + } else { + long wait_time = wpt->GetCreationTime().toTime_t() - last_time; + if (wait_time > 0) { + gb_sleep(wait_time * 1000000); + } + } + } + last_time = wpt->GetCreationTime().toTime_t(); + } + + lat = degrees2ddmm(wpt->latitude); + lon = degrees2ddmm(wpt->longitude); + + time_t ct = wpt->GetCreationTime().toTime_t(); + tm = gmtime(&ct); + if (tm) { + hms = tm->tm_hour * 10000 + tm->tm_min * 100 + tm->tm_sec; + ymd = tm->tm_mday * 10000 + tm->tm_mon * 100 + tm->tm_year; + } else { + hms = 0; + ymd = 0; + } + + switch (wpt->fix) { + case fix_dgps: + fix='2'; + break; + case fix_3d: + case fix_2d: + fix='1'; + break; + case fix_pps: + fix='3'; + break; + default: + fix='0'; + } + + if (opt_gprmc) { + snprintf(obuf, sizeof(obuf), "GPRMC,%010.3f,%c,%08.3f,%c,%09.3f,%c,%.2f,%.2f,%06d,,", + (double) hms + (wpt->GetCreationTime().time().msec() / 1000.0), + fix=='0' ? 'V' : 'A', + fabs(lat), lat < 0 ? 'S' : 'N', + fabs(lon), lon < 0 ? 'W' : 'E', + WAYPT_HAS(wpt, speed) ? MPS_TO_KNOTS(wpt->speed):(0), + WAYPT_HAS(wpt, course) ? (wpt->course):(0), + (int) ymd); + cksum = nmea_cksum(obuf); + + /* GISTeq doesn't care about the checksum, but wants this prefixed, so + * we can write it with abandon. + */ + if (opt_gisteq) { + gbfprintf(file_out, "---,"); + } + gbfprintf(file_out, "$%s*%02X\n", obuf, cksum); + } + if (opt_gpgga) { + snprintf(obuf, sizeof(obuf), "GPGGA,%010.3f,%08.3f,%c,%09.3f,%c,%c,%02d,%.1f,%.3f,M,%.1f,M,,", + (double) hms + (wpt->GetCreationTime().time().msec() / 1000.0), + fabs(lat), lat < 0 ? 'S' : 'N', + fabs(lon), lon < 0 ? 'W' : 'E', + fix, + (wpt->sat>0)?(wpt->sat):(0), + (wpt->hdop>0)?(wpt->hdop):(0.0), + wpt->altitude == unknown_alt ? 0 : wpt->altitude, + WAYPT_HAS(wpt, geoidheight)? (wpt->geoidheight) : (0)); /* TODO: we could look up the geoidheight if needed */ + cksum = nmea_cksum(obuf); + gbfprintf(file_out, "$%s*%02X\n", obuf, cksum); + } + if ((opt_gpvtg) && (WAYPT_HAS(wpt, course) || WAYPT_HAS(wpt, speed))) { + snprintf(obuf,sizeof(obuf),"GPVTG,%.3f,T,0,M,%.3f,N,%.3f,K", + WAYPT_HAS(wpt, course) ? (wpt->course):(0), + WAYPT_HAS(wpt, speed) ? MPS_TO_KNOTS(wpt->speed):(0), + WAYPT_HAS(wpt, speed) ? MPS_TO_KPH(wpt->speed):(0)); + + cksum = nmea_cksum(obuf); + gbfprintf(file_out, "$%s*%02X\n", obuf, cksum); + } + + if ((opt_gpgsa) && (wpt->fix!=fix_unknown)) { + + switch (wpt->fix) { + case fix_dgps: + /* or */ + case fix_3d: + fix='3'; + break; + case fix_2d: + fix='2'; + break; + default: + fix=0; + } + snprintf(obuf,sizeof(obuf),"GPGSA,A,%c,,,,,,,,,,,,,%.1f,%.1f,%.1f", + fix, + (wpt->pdop>0)?(wpt->pdop):(0), + (wpt->hdop>0)?(wpt->hdop):(0), + (wpt->vdop>0)?(wpt->vdop):(0)); + cksum = nmea_cksum(obuf); + gbfprintf(file_out, "$%s*%02X\n", obuf, cksum); + } + gbfflush(file_out); + } + + static void + nmea_write(void) + { + waypt_disp_all(nmea_wayptpr); + track_disp_all(nmea_track_init, NULL, nmea_trackpt_pr); + } + + static void + nmea_wr_posn_init(const char* fname) + { + nmea_wr_init(fname); + } + + static void + nmea_wr_posn(Waypoint* wpt) + { + nmea_trackpt_pr(wpt); + } + + static void + nmea_wr_posn_deinit(void) + { + // nmea_wr_deinit(); + } + + + ff_vecs_t nmea_vecs = { + ff_type_file, + { + (ff_cap)(ff_cap_read | ff_cap_write), + (ff_cap)(ff_cap_read | ff_cap_write), + ff_cap_none + }, + nmea_rd_init, + nmea_wr_init, + nmea_rd_deinit, + nmea_wr_deinit, + nmea_read, + nmea_write, + NULL, + nmea_args, + CET_CHARSET_ASCII, 0, /* CET-REVIEW */ + { + nmea_rd_posn_init, nmea_rd_posn, nmea_rd_deinit, + nmea_wr_posn_init, nmea_wr_posn, nmea_wr_posn_deinit + } + }; + + /* + * If we later decide to implement a "real" Sirf module, this code should + * go there. For now, we try a kind of heavy handed thing - if we don't + * see NMEA-isms from the device, we'll go on the premise that it MAY be + * a SiRF Star device and send it the "speak NMEA, please" command. + */ + + static void + sirf_write(unsigned char* buf) + { + int i, chksum = 0; + int len = buf[2] << 8 | buf[3]; + + for (i = 0; i < len; i++) { + chksum += buf[4 + i]; + } + chksum &= 0x7fff; + + buf[len + 4] = chksum >> 8; + buf[len + 5] = chksum & 0xff; + + gbser_write(gbser_handle, buf, len + 8); /* 4 at front, 4 at back */ + } + + static + void reset_sirf_to_nmea(int br) + { + static unsigned char pkt[] = {0xa0, 0xa2, 0x00, 0x18, + 0x81, 0x02, + 0x01, 0x01, /* GGA */ + 0x00, 0x00, /* suppress GLL */ + 0x01, 0x00, /* suppress GSA */ + 0x05, 0x00, /* suppress GSV */ + 0x01, 0x01, /* use RMC for date*/ + 0x00, 0x00, /* suppress VTG */ + 0x00, 0x01, /* output rate */ + 0x00, 0x01, /* unused recommended values */ + 0x00, 0x01, + 0x00, 0x01, /* ZDA */ + 0x12, 0xc0, /* 4800 bps */ + 0x00, 0x00, /* checksum */ + 0xb0, 0xb3 + }; /* packet end */ + /* repopulate bit rate */ + pkt[26] = br >> 8; + pkt[27] = br & 0xff; + + sirf_write(pkt); + gb_sleep(250 * 1000); + gbser_flush(gbser_handle); + }